
#include "apply.h"

cf_t cf;
lpf_t lpf;
lpf2_t lpf2;
blpf2_t blpf2;
/**
 * @brief  滤波器初始化
 * @retval void
 */
void filter_init(void)
{
	// 互补滤波
	cf_init_for_fr(&cf, 20, DT);

	// 一阶低通滤波
	lpf_init(&lpf, 20, DT);

	// 二阶低通滤波器
	lpf2_init(&lpf2, 20, DT);

	// 二阶巴特沃斯滤波器
	blpf2_init(&blpf2, 8, DT);
}

/**
 * @brief  滤波器任务
 * @param  raw : 原始值
 * @param  t   : 时间
 * @retval void
 */
void filter_task(float raw, int t)
{
	ege_draw_point(t, raw, RED);
	
	float cf_filt = cf_apply(&cf, raw);
	ege_draw_point(t, cf_filt, GREEN);

	// float lpf_filt = lpf_apply(&lpf, raw);
	// ege_draw_point(t, lpf_filt, YELLOW);

	// float lpf2_filt = lpf2_apply(&lpf2, raw);
	// ege_draw_point(t, lpf2_filt, WHITE);

	float blpf2_filt = blpf2_apply(&blpf2, raw);
	ege_draw_point(t, blpf2_filt, YELLOW);
}
